#ifndef __DRV_GM6020_H
#define __DRV_GM6020_H
#include "main.h"
#include "bsp_can.h"
#include "bsp_tim.h"
#include "drv_algo.h"

typedef struct
{
  uint16_t can_id;
  uint8_t in_id;
  int16_t  set_voltage;
  uint16_t rotor_angle;
  int16_t  rotor_speed;
  int16_t  torque_current;
  uint8_t  temp;
} motor_gm6020_t;

extern algo_pid_t pid_spd_to_vol;
extern algo_pid_t pid_ang_to_spd;

void GM6020_UpdateMotor(motor_gm6020_t* gm_ptr, uint8_t* arrData);
void GM6020_Init(motor_gm6020_t* gm_ptr, uint16_t canId);
float GM6020_CalcOutput(motor_gm6020_t* gm_ptr, uint16_t target_angle);
void GM6020_DrivePosMode(motor_gm6020_t* gm_ptr, float angle);
uint8_t GM6020_GotoZero(void);

#endif
